module weno_rhombus_mod
  use recon_type_mod
  use recon_math_mod
  use poly_utils_mod

  implicit none

  !! 1: i,j,x^?,y^?
  !! 2: cell
  !! 3: sub-stencil
  integer, dimension(:,:), allocatable, target :: stencil_3
  integer, dimension(:,:), allocatable, target :: stencil_5
  integer, dimension(:,:), allocatable, target :: stencil_7
  
  integer, dimension(:,:,:), allocatable, target :: substencil_3
  integer, dimension(:,:,:), allocatable, target :: substencil_5
  integer, dimension(:,:,:), allocatable, target :: substencil_7
    
contains

  recursive subroutine weno_rhombus_init(this, nd, sw, swx, swy, swz, xc, yc, zc, dx, dy, dz, ic, jc, kc, is, ie, js, je, ks, ke, mask, id)

    class(recon_type), intent(inout) :: this
    integer , intent(in), optional :: nd
    integer , intent(in), optional :: sw
    integer , intent(in), optional :: swx
    integer , intent(in), optional :: swy
    integer , intent(in), optional :: swz
    real(16), intent(in), optional :: xc(:)
    real(16), intent(in), optional :: yc(:)
    real(16), intent(in), optional :: zc(:)
    real(16), intent(in), optional :: dx
    real(16), intent(in), optional :: dy
    real(16), intent(in), optional :: dz
    integer , intent(in), optional :: ic
    integer , intent(in), optional :: jc
    integer , intent(in), optional :: kc
    integer , intent(in), optional :: is
    integer , intent(in), optional :: ie
    integer , intent(in), optional :: js
    integer , intent(in), optional :: je
    integer , intent(in), optional :: ks
    integer , intent(in), optional :: ke
    integer , intent(in), optional :: mask(:,:,:) ! Cell mask
    integer , intent(in), optional :: id

    real(16), allocatable :: x(:), y(:)
    integer i, j, k, isub, iCell, sub_swx, sub_swy, sub_is, sub_ie, sub_js, sub_je
    integer it, jt, kt
    integer max_ngb, ingb, m, n
    
    if(this%id==0)then
      if(.not.allocated(stencil_3   ))allocate(stencil_3   (4,5 ))
      if(.not.allocated(stencil_5   ))allocate(stencil_5   (4,13))
      if(.not.allocated(stencil_7   ))allocate(stencil_7   (4,25))
      if(.not.allocated(substencil_3))allocate(substencil_3(4,3,4 ))
      if(.not.allocated(substencil_5))allocate(substencil_5(4,5,9 ))
      if(.not.allocated(substencil_7))allocate(substencil_7(4,7,20))
      
                                  stencil_3(:,1) = [2,3,0,2]                             !       y^2
      stencil_3(:,2) = [1,2,1,0]; stencil_3(:,3) = [2,2,0,0]; stencil_3(:,4) = [3,2,2,0] !   x    1    x^2
                                  stencil_3(:,5) = [2,1,0,1]                             !        y
      
                                                                stencil_5(:, 1) = [3,5,0,4]                                                            !                     y^4
                                   stencil_5(:, 2) = [2,4,1,2]; stencil_5(:, 3) = [3,4,0,3]; stencil_5(:, 4) = [4,4,2,2]                               !          x y^2      y^3      x^2 y^2
      stencil_5(:, 5) = [1,3,2,0]; stencil_5(:, 6) = [2,3,1,0]; stencil_5(:, 7) = [3,3,0,0]; stencil_5(:, 8) = [4,3,3,0]; stencil_5(:, 9) = [5,3,4,0]  !  x^2     x           1       x^3          x^4
                                   stencil_5(:,10) = [2,2,1,1]; stencil_5(:,11) = [3,2,0,1]; stencil_5(:,12) = [4,2,2,1]                               !          x y         y       x^2 y
                                                                stencil_5(:,13) = [3,1,0,2]                                                            !                     y^2
                                   
                                                                                             stencil_7(:, 1) = [4,7,0,6]                                 
                                                                stencil_7(:, 2) = [3,6,1,4]; stencil_7(:, 3) = [4,6,0,4]; stencil_7(:, 4) = [5,6,2,4]                      
                                   stencil_7(:, 5) = [2,5,3,2]; stencil_7(:, 6) = [3,5,1,2]; stencil_7(:, 7) = [4,5,0,2]; stencil_7(:, 8) = [5,5,2,2]; stencil_7(:, 9) = [6,5,4,2]           
      stencil_7(:,10) = [1,4,5,0]; stencil_7(:,11) = [2,4,3,0]; stencil_7(:,12) = [3,4,1,0]; stencil_7(:,13) = [4,4,0,0]; stencil_7(:,14) = [5,4,2,0]; stencil_7(:,15) = [6,4,4,0]; stencil_7(:,16) = [7,4,6,0]
                                   stencil_7(:,17) = [2,3,3,1]; stencil_7(:,18) = [3,3,1,1]; stencil_7(:,19) = [4,3,0,1]; stencil_7(:,20) = [5,3,2,1]; stencil_7(:,21) = [6,3,4,1]           
                                                                stencil_7(:,22) = [3,2,1,3]; stencil_7(:,23) = [4,2,0,3]; stencil_7(:,24) = [5,2,2,3]                      
                                                                                             stencil_7(:,25) = [4,1,0,5]     
      
      substencil_3(:,1,1) = [2,2,0,0]; substencil_3(:,2,1) = [1,2,1,0]; substencil_3(:,3,1) = [2,1,0,1] ! sub-stencil 1
      substencil_3(:,1,2) = [2,2,0,0]; substencil_3(:,2,2) = [3,2,1,0]; substencil_3(:,3,2) = [2,1,0,1] ! sub-stencil 2
      substencil_3(:,1,3) = [2,2,0,0]; substencil_3(:,2,3) = [3,2,1,0]; substencil_3(:,3,3) = [2,3,0,1] ! sub-stencil 3
      substencil_3(:,1,4) = [2,2,0,0]; substencil_3(:,2,4) = [1,2,1,0]; substencil_3(:,3,4) = [2,3,0,1] ! sub-stencil 4                                                   
                         
      substencil_5(:,1,1) = [3,3,0,0]; substencil_5(:,2,1) = [2,3,1,0]; substencil_5(:,3,1) = [4,3,0,1]; substencil_5(:,4,1) = [3,2,2,0]; substencil_5(:,5,1) = [3,4,0,2]
      substencil_5(:,1,2) = [2,3,0,0]; substencil_5(:,2,2) = [1,3,1,0]; substencil_5(:,3,2) = [3,3,0,1]; substencil_5(:,4,2) = [2,2,2,0]; substencil_5(:,5,2) = [2,4,0,2]
      substencil_5(:,1,3) = [3,2,0,0]; substencil_5(:,2,3) = [2,2,1,0]; substencil_5(:,3,3) = [4,2,0,1]; substencil_5(:,4,3) = [3,3,2,0]; substencil_5(:,5,3) = [3,1,0,2]
      substencil_5(:,1,4) = [4,3,0,0]; substencil_5(:,2,4) = [3,3,1,0]; substencil_5(:,3,4) = [5,3,0,1]; substencil_5(:,4,4) = [4,2,2,0]; substencil_5(:,5,4) = [4,4,0,2]
      substencil_5(:,1,5) = [3,4,0,0]; substencil_5(:,2,5) = [2,4,1,0]; substencil_5(:,3,5) = [4,4,0,1]; substencil_5(:,4,5) = [3,3,2,0]; substencil_5(:,5,5) = [3,5,0,2]
      substencil_5(:,1,6) = [3,5,0,0]; substencil_5(:,2,6) = [3,4,1,0]; substencil_5(:,3,6) = [3,3,0,1]; substencil_5(:,4,6) = [2,3,2,0]; substencil_5(:,5,6) = [4,3,0,2]
      substencil_5(:,1,7) = [1,3,0,0]; substencil_5(:,2,7) = [2,3,1,0]; substencil_5(:,3,7) = [3,3,0,1]; substencil_5(:,4,7) = [3,2,2,0]; substencil_5(:,5,7) = [3,4,0,2]
      substencil_5(:,1,8) = [3,1,0,0]; substencil_5(:,2,8) = [3,2,1,0]; substencil_5(:,3,8) = [3,3,0,1]; substencil_5(:,4,8) = [2,3,2,0]; substencil_5(:,5,8) = [4,3,0,2]
      substencil_5(:,1,9) = [5,3,0,0]; substencil_5(:,2,9) = [4,3,1,0]; substencil_5(:,3,9) = [3,3,0,1]; substencil_5(:,4,9) = [3,4,2,0]; substencil_5(:,5,9) = [3,2,0,2]
      
      substencil_7(:,1, 1) = [1,4,0,0]; substencil_7(:,2, 1) = [2,4,1,0]; substencil_7(:,3, 1) = [3,4,2,0]; substencil_7(:,4, 1) = [4,4,3,0]; substencil_7(:,5, 1) = [4,1,0,1]; substencil_7(:,6, 1) = [4,2,0,2]; substencil_7(:,7, 1) = [4,3,0,3]
      substencil_7(:,1, 2) = [4,4,0,0]; substencil_7(:,2, 2) = [5,4,1,0]; substencil_7(:,3, 2) = [6,4,2,0]; substencil_7(:,4, 2) = [7,4,3,0]; substencil_7(:,5, 2) = [4,1,0,1]; substencil_7(:,6, 2) = [4,2,0,2]; substencil_7(:,7, 2) = [4,3,0,3]
      substencil_7(:,1, 3) = [4,4,0,0]; substencil_7(:,2, 3) = [5,4,1,0]; substencil_7(:,3, 3) = [6,4,2,0]; substencil_7(:,4, 3) = [7,4,3,0]; substencil_7(:,5, 3) = [4,5,0,1]; substencil_7(:,6, 3) = [4,6,0,2]; substencil_7(:,7, 3) = [4,7,0,3]
      substencil_7(:,1, 4) = [1,4,0,0]; substencil_7(:,2, 4) = [2,4,1,0]; substencil_7(:,3, 4) = [3,4,2,0]; substencil_7(:,4, 4) = [4,4,3,0]; substencil_7(:,5, 4) = [4,5,0,1]; substencil_7(:,6, 4) = [4,6,0,2]; substencil_7(:,7, 4) = [4,7,0,3]
      substencil_7(:,1, 5) = [3,3,0,0]; substencil_7(:,2, 5) = [3,4,1,0]; substencil_7(:,3, 5) = [3,5,2,0]; substencil_7(:,4, 5) = [4,3,0,1]; substencil_7(:,5, 5) = [4,4,0,2]; substencil_7(:,6, 5) = [4,5,1,1]; substencil_7(:,7, 5) = [5,4,1,2]
      substencil_7(:,1, 6) = [3,5,0,0]; substencil_7(:,2, 6) = [4,5,1,0]; substencil_7(:,3, 6) = [5,5,2,0]; substencil_7(:,4, 6) = [3,4,0,1]; substencil_7(:,5, 6) = [4,4,0,2]; substencil_7(:,6, 6) = [5,4,1,1]; substencil_7(:,7, 6) = [4,3,2,1]
      substencil_7(:,1, 7) = [5,3,0,0]; substencil_7(:,2, 7) = [5,4,1,0]; substencil_7(:,3, 7) = [5,5,2,0]; substencil_7(:,4, 7) = [4,3,0,1]; substencil_7(:,5, 7) = [4,4,0,2]; substencil_7(:,6, 7) = [4,5,1,1]; substencil_7(:,7, 7) = [3,4,1,2]
      substencil_7(:,1, 8) = [3,3,0,0]; substencil_7(:,2, 8) = [4,3,1,0]; substencil_7(:,3, 8) = [5,3,2,0]; substencil_7(:,4, 8) = [3,4,0,1]; substencil_7(:,5, 8) = [4,4,0,2]; substencil_7(:,6, 8) = [5,4,1,1]; substencil_7(:,7, 8) = [4,5,2,1]
      substencil_7(:,1, 9) = [1,4,0,0]; substencil_7(:,2, 9) = [2,4,1,0]; substencil_7(:,3, 9) = [3,4,2,0]; substencil_7(:,4, 9) = [4,4,3,0]; substencil_7(:,5, 9) = [5,4,4,0]; substencil_7(:,6, 9) = [2,3,0,1]; substencil_7(:,7, 9) = [2,5,0,2]
      substencil_7(:,1,10) = [4,1,0,0]; substencil_7(:,2,10) = [4,2,1,0]; substencil_7(:,3,10) = [4,3,2,0]; substencil_7(:,4,10) = [4,4,0,1]; substencil_7(:,5,10) = [4,5,0,2]; substencil_7(:,6,10) = [3,2,0,3]; substencil_7(:,7,10) = [5,2,0,4]
      substencil_7(:,1,11) = [3,4,0,0]; substencil_7(:,2,11) = [4,4,1,0]; substencil_7(:,3,11) = [5,4,2,0]; substencil_7(:,4,11) = [6,4,3,0]; substencil_7(:,5,11) = [7,4,4,0]; substencil_7(:,6,11) = [6,3,0,1]; substencil_7(:,7,11) = [6,5,0,2]
      substencil_7(:,1,12) = [4,3,0,0]; substencil_7(:,2,12) = [4,4,1,0]; substencil_7(:,3,12) = [4,5,2,0]; substencil_7(:,4,12) = [4,6,0,1]; substencil_7(:,5,12) = [4,7,0,2]; substencil_7(:,6,12) = [3,6,0,3]; substencil_7(:,7,12) = [5,6,0,4]
      substencil_7(:,1,13) = [3,2,0,0]; substencil_7(:,2,13) = [3,3,1,0]; substencil_7(:,3,13) = [3,4,2,0]; substencil_7(:,4,13) = [3,5,0,1]; substencil_7(:,5,13) = [3,6,0,2]; substencil_7(:,6,13) = [4,4,0,3]; substencil_7(:,7,13) = [5,4,0,4]
      substencil_7(:,1,14) = [2,3,0,0]; substencil_7(:,2,14) = [3,3,1,0]; substencil_7(:,3,14) = [4,3,2,0]; substencil_7(:,4,14) = [5,3,3,0]; substencil_7(:,5,14) = [6,3,4,0]; substencil_7(:,6,14) = [4,4,0,1]; substencil_7(:,7,14) = [4,5,0,2]
      substencil_7(:,1,15) = [3,4,0,0]; substencil_7(:,2,15) = [4,4,1,0]; substencil_7(:,3,15) = [5,4,2,0]; substencil_7(:,4,15) = [5,2,0,1]; substencil_7(:,5,15) = [5,3,0,2]; substencil_7(:,6,15) = [5,5,0,3]; substencil_7(:,7,15) = [5,6,0,4]
      substencil_7(:,1,16) = [2,5,0,0]; substencil_7(:,2,16) = [3,5,1,0]; substencil_7(:,3,16) = [4,5,2,0]; substencil_7(:,4,16) = [5,5,3,0]; substencil_7(:,5,16) = [6,5,4,0]; substencil_7(:,6,16) = [4,3,0,1]; substencil_7(:,7,16) = [4,4,0,2]
      substencil_7(:,1,17) = [3,4,0,0]; substencil_7(:,2,17) = [4,4,1,0]; substencil_7(:,3,17) = [5,4,2,0]; substencil_7(:,4,17) = [4,5,0,1]; substencil_7(:,5,17) = [3,6,0,2]; substencil_7(:,6,17) = [4,6,1,1]; substencil_7(:,7,17) = [5,6,2,1]
      substencil_7(:,1,18) = [3,4,0,0]; substencil_7(:,2,18) = [4,4,1,0]; substencil_7(:,3,18) = [5,4,2,0]; substencil_7(:,4,18) = [4,3,0,1]; substencil_7(:,5,18) = [3,2,0,2]; substencil_7(:,6,18) = [4,2,1,1]; substencil_7(:,7,18) = [5,2,2,1]
      substencil_7(:,1,19) = [2,3,0,0]; substencil_7(:,2,19) = [2,4,1,0]; substencil_7(:,3,19) = [2,5,2,0]; substencil_7(:,4,19) = [3,4,0,1]; substencil_7(:,5,19) = [4,3,0,2]; substencil_7(:,6,19) = [4,4,1,1]; substencil_7(:,7,19) = [4,5,1,2]
      substencil_7(:,1,20) = [6,3,0,0]; substencil_7(:,2,20) = [6,4,1,0]; substencil_7(:,3,20) = [6,5,2,0]; substencil_7(:,4,20) = [5,4,0,1]; substencil_7(:,5,20) = [4,3,0,2]; substencil_7(:,6,20) = [4,4,1,1]; substencil_7(:,7,20) = [4,5,1,2]
    endif
  
    call this%clear()
    
    if (present(sw)) then
      this%sw  = sw
      this%swx = sw
      this%swy = sw
      this%swz = 1
    else if (present(swx) .and. present(swy)) then
      this%swx = swx
      this%swy = swy
      this%swz = 1
    end if
    this%nd      = nd
    this%max_ngb = 3**nd-1
    
    this%dx = dx
    this%dy = dy
    if(present(dz))then
      this%dz = dz
    else
      this%dz = 1
    endif
    
    max_ngb = this%max_ngb

    if (present(id)) this%id = id

    if(present(is))then; this%is = is; else; this%is = 1       ; endif
    if(present(ie))then; this%ie = ie; else; this%ie = this%swx; endif
    if(present(js))then; this%js = js; else; this%js = 1       ; endif
    if(present(je))then; this%je = je; else; this%je = this%swy; endif
    if(present(ks))then; this%ks = ks; else; this%ks = 1       ; endif
    if(present(ke))then; this%ke = ke; else; this%ke = 1       ; endif
    
    this%eps     = 1.e-15
    this%eps_r16 = 1.e-18

    allocate(this%cell_mask(this%is:this%ie,this%js:this%je,this%ks:this%ke))
    this%cell_mask = 0

    if (present(mask)) then
      this%cell_mask = mask
    else
      this%cell_mask = 1
    end if
    
    if(this%id == 0)then
      ! Set TCI
      do k = this%ks, this%ke
        do j = this%js, this%je
          do i = this%is, this%ie
            it = i - ( this%swx + 1 ) / 2
            jt = j - ( this%swy + 1 ) / 2
            kt = k - ( this%swz + 1 ) / 2
            this%cell_mask(i,j,k) = merge( 1, 0, abs(it) + abs(jt) <= this%sw )
          enddo
        enddo
      enddo
      allocate(this%cell_ngb     (nd,max_ngb,this%swx,this%swy,this%swz))
      allocate(this%cell_ngb_size(           this%swx,this%swy,this%swz))
      do k = 1, this%swz
        do j = 1, this%swy
          do i = 1, this%swx
            if (this%cell_mask(i,j,1) == 1) then
              ingb = 0
              do n = max(this%js, j - 1), min(this%je, j + 1)
                do m = max(this%is, i - 1), min(this%ie, i + 1)
                  if (this%cell_mask(m,n,1) == 1 .and. m /= i .and. n /= j) then
                    ingb = ingb + 1
                    this%cell_ngb(1,ingb,i,j,k) = m
                    this%cell_ngb(2,ingb,i,j,k) = n
                  end if
                end do
              end do
              this%cell_ngb_size(i,j,k) = ingb
            else
              this%cell_ngb_size(i,j,k) = 0
            end if
          end do
        end do
      enddo
      
      ! Set stencil
      select case (this%sw)
      case (3)
        this%ns = size(substencil_3, 3)
        this%ijxy => stencil_3
      case (5)
        this%ns = size(substencil_5, 3)
        this%ijxy => stencil_5
      case (7)
        this%ns = size(substencil_7, 3)
        this%ijxy => stencil_7
      end select
    end if
    this%nc = size(this%ijxy, 2)

    ! Set cell coordinates.
    allocate(this%xc(this%nc))
    allocate(this%yc(this%nc))
    allocate(x(this%is:this%ie))
    allocate(y(this%js:this%je))
    if (present(xc) .and. present(yc)) then
      x = xc
      y = yc
    else
      ! Set coordinates of cells on the large stencil with origin at center.
      do i = this%is, this%ie
        x(i) = -int(this%swx / 2) + i - 1
      end do
      do j = this%js, this%je
        y(j) = -int(this%swy / 2) + j - 1
      end do
    end if
    do iCell = 1, this%nc
      this%xc(iCell) = x(this%ijxy(1,iCell))
      this%yc(iCell) = y(this%ijxy(2,iCell))
    end do

    ! Initialize sub-stencils.
    if (this%id == 0) then
      this%nterm = this%sw ! Caution here
      allocate(this%subs(this%ns))
      do isub = 1, this%ns
        ! init sub procedures
        this%subs(isub)%init                   => weno_rhombus_init
        this%subs(isub)%calc_recon_matrix      => weno_rhombus_calc_recon_matrix     
        this%subs(isub)%calc_ideal_coefs       => weno_rhombus_calc_ideal_coefs      
        this%subs(isub)%reconstruct            => weno_rhombus_reconstruct           
        this%subs(isub)%trouble_cell_indicator => weno_rhombus_trouble_cell_indicator
        select case (this%sw)
        case (3)
          sub_is  = minval(substencil_3(1,:,isub)); sub_ie  = maxval(substencil_3(1,:,isub))
          sub_js  = minval(substencil_3(2,:,isub)); sub_je  = maxval(substencil_3(2,:,isub))
          sub_swx = sub_ie - sub_is + 1; sub_swy = sub_je - sub_js + 1
          this%subs(isub)%ijxy => substencil_3(:,:,isub)
        case (5)
          sub_is  = minval(substencil_5(1,:,isub)); sub_ie  = maxval(substencil_5(1,:,isub))
          sub_js  = minval(substencil_5(2,:,isub)); sub_je  = maxval(substencil_5(2,:,isub))
          sub_swx = sub_ie - sub_is + 1; sub_swy = sub_je - sub_js + 1
          this%subs(isub)%ijxy => substencil_5(:,:,isub)
        case (7)
          sub_is  = minval(substencil_7(1,:,isub)); sub_ie  = maxval(substencil_7(1,:,isub))
          sub_js  = minval(substencil_7(2,:,isub)); sub_je  = maxval(substencil_7(2,:,isub))
          sub_swx = sub_ie - sub_is + 1; sub_swy = sub_je - sub_js + 1
          this%subs(isub)%ijxy => substencil_7(:,:,isub)
        end select
        
        call this%subs(isub)%init(nd=nd, swx=sub_swx, swy=sub_swy, xc=x(sub_is:sub_ie), yc=y(sub_js:sub_je), &
                                  dx=this%dx, dy=this%dy, dz=this%dz, is=sub_is, ie=sub_ie, js=sub_js, je=sub_je, id=isub)
      
        select case (this%sw)
        case (3)
          this%subs(isub)%smooth_indicator => smooth_indicator_rhombus_2
        case default
          print*,'WENO_rhombus does not support sw>3 cases for now, reset recon_h_order = 3 in namelist'
          stop
        end select
      end do
      
      ! Set index map.
      allocate(this%ijk_to_1d(this%sw,this%sw,1)); this%ijk_to_1d = 0
      do iCell = 1, this%nc
        this%ijk_to_1d(this%ijxy(1,iCell),this%ijxy(2,iCell),1) = iCell
      end do
    end if

    this%initialized = .true.

  end subroutine weno_rhombus_init

  subroutine weno_rhombus_calc_recon_matrix(this, ierr)

    class(recon_type), intent(inout) :: this
    integer, intent(out) :: ierr

    real(16), allocatable, dimension(:,:) :: A, iA
    integer i, j, it, ipt, iterm, dxn, dyn

    ierr = 0

    if (allocated(this%iA        )) deallocate(this%iA        )
    if (allocated(this%poly_r16  )) deallocate(this%poly_r16  )
    if (allocated(this%recon_mtx )) deallocate(this%recon_mtx )
    if (allocated(this%dpoly_r16 )) deallocate(this%dpoly_r16 )
    if (allocated(this%drecon_mtx)) deallocate(this%drecon_mtx)

    allocate(this%iA        (this%nc ,this%nc))
    allocate(this%poly_r16  (this%npt,this%nc))
    allocate(this%recon_mtx (this%npt,this%nc))
    allocate(this%dpoly_r16 (this%npt,this%nc,0:this%swx-1,0:this%swy-1,0:this%swz-1))
    allocate(this%drecon_mtx(this%npt,this%nc,0:this%swx-1,0:this%swy-1,0:this%swz-1))

    allocate( A(this%nc,this%nc))
    allocate(iA(this%nc,this%nc))

    do ipt = 1, this%npt
      iterm = 1
      do it = 1, this%nc
        call calc_monomial(this%x(ipt), this%y(ipt), this%ijxy(3,it), this%ijxy(4,it), this%poly_r16(ipt,it))
        do dyn = 0, this%swy - 1
          do dxn = 0, this%swx - 1
            call calc_deriv_monomial(this%x(ipt), this%y(ipt), this%ijxy(3,it), this%ijxy(4,it), dxn, dyn, this%swx, this%swy, this%dpoly_r16(ipt,iterm,dxn,dyn,0))
            this%dpoly_r16(ipt,iterm,dxn,dyn,0) = this%dpoly_r16(ipt,iterm,dxn,dyn,0) / ( this%dx**dxn * this%dy**dyn )
          end do
        end do
        iterm = iterm + 1
      end do
    end do
    
    call calc_poly_integral_matrix(this%nc, this%nc, this%ijxy(3:4,:), this%xc, this%yc, A)
    call inverse_matrix(A, iA, ierr)
    if (ierr /= 0) then
      deallocate(A, iA)
      return
    end if

    this%recon_mtx = matmul(this%poly_r16, iA)
    
    do dyn = 0, this%swy - 1
      do dxn = 0, this%swx - 1
        this%drecon_mtx(:,:,dxn,dyn,0) = matmul(this%dpoly_r16(:,:,dxn,dyn,0), iA)
      end do
    end do

    deallocate(A, iA)

  end subroutine weno_rhombus_calc_recon_matrix

  subroutine weno_rhombus_calc_ideal_coefs(this, ierr)

    class(recon_type), intent(inout) :: this
    integer, intent(out) :: ierr

    ! Local double double arrays for preserving precision.
    real(16), allocatable, dimension(:,:) :: A, ATA, iATA
    integer it, k, ipt, i, j
    
    real(16), parameter :: theta = 3

    ierr = 0

    if (.not. this%initialized) then
      ierr = 1
      return
    end if

    do k = 1, this%ns
      call this%subs(k)%calc_recon_matrix(ierr)
      if (ierr /= 0) return
    end do
    call this%calc_recon_matrix(ierr)
    if (ierr /= 0) return

    if (allocated(this%gamma)) deallocate(this%gamma)
    allocate(this%gamma(this%ns,this%npt))

    allocate(    A(this%nc,this%ns )); A = 0
    allocate(  ATA(this%ns,this%ns ))
    allocate( iATA(this%ns,this%ns )); iATA = 0
    do ipt = 1, this%npt
      substencil: do k = 1, this%ns
        monomial: do it = 1, this%subs(k)%nc
          i = this%subs(k)%ijxy(1,it)
          j = this%subs(k)%ijxy(2,it)
          A(this%ijk_to_1d(i,j,1),k) = this%subs(k)%recon_mtx(ipt,it)
        end do monomial
      end do substencil

      if( all( A(this%nc/2+1,:)==1 ) )then
        this%gamma(:,ipt) = 1. / this%ns
      else
        ATA = matmul(transpose(A), A)
        do i = 1, this%ns
          ATA(i,i) = ATA(i,i) + this%eps_r16
        end do
        call inverse_matrix(ATA, iATA, ierr)
        if (ierr /= 0) return
        this%gamma(:,ipt) = matmul(matmul(iATA, transpose(A)), this%recon_mtx(ipt,:))
        ! Set near-zero values to zero exactly.
        do k = 1, this%ns
          if (abs(this%gamma(k,ipt)) < 1.0e-15) this%gamma(k,ipt) = 0
        end do
  
        if (abs(sum(matmul(A, this%gamma(:,ipt)) - this%recon_mtx(ipt,:))) > 1.0e-15) then
          write(*, *) 'Residual: ', abs(sum(matmul(A, this%gamma(:,ipt)) - this%recon_mtx(ipt,:)))
          write(*, *) 'Gamma: ', this%gamma(:,ipt)
          ierr = 3
          this%gamma = 0
          return
        end if
      endif
      ! print*,ipt,real(this%gamma(:,ipt),4),abs(sum(matmul(A, this%gamma(:,ipt)) - this%recon_mtx(ipt,:)))
    end do

    deallocate(A, ATA, iATA)

    if (allocated(this%rp)) deallocate(this%rp)
    allocate(this%rp(this%ns,this%npt))
    if (allocated(this%rn)) deallocate(this%rn)
    allocate(this%rn(this%ns,this%npt))
    if (allocated(this%sigmap)) deallocate(this%sigmap)
    allocate(this%sigmap(this%npt))
    if (allocated(this%sigman)) deallocate(this%sigman)
    allocate(this%sigman(this%npt))

    this%rp = 0.5d0 * (this%gamma + theta * abs(this%gamma))
    this%rn = this%rp - this%gamma

    this%sigmap = sum(this%rp, 1)
    this%sigman = sum(this%rn, 1)

    do ipt = 1, this%npt
      this%rp(:,ipt) = this%rp(:,ipt) / this%sigmap(ipt)
      this%rn(:,ipt) = this%rn(:,ipt) / this%sigman(ipt)
    end do

  end subroutine weno_rhombus_calc_ideal_coefs

  subroutine weno_rhombus_reconstruct(this, fi, fo, TCI, ierr)

    class(recon_type), intent(inout) :: this
    real(8), intent(in ) :: fi(:,:,:) ! Cell averaged function values
    real(8), intent(out) :: fo(:)     ! Reconstructed function values on evaluation points
    logical, intent(in ), optional :: TCI ! Trouble Cell Indicator(TCI), 1 for existing TC(use WENO), 0 for no TC(use poly)
    integer, intent(out), optional :: ierr
    
    integer i,j,k
    integer isub, ipt, iCell
    real(8) a     (this%nterm,this%ns)
    real(8) beta  (this%ns)
    real(8) alpha (this%ns)

    real(8) fs(this%ns,this%npt)
    real(8) alpha_p(this%ns)
    real(8) alpha_n(this%ns)
    
    real(8) f    (this%nc)
    real(8) f_sub(this%nc,this%ns)
    
#ifdef DEBUG
    if(present(ierr))then
      ierr = 0
      if (size(fi) /= this%nc) then
        if (present(ierr)) ierr = 1
        return
      end if
      if (size(fo) /= this%npt) then
        if (present(ierr)) ierr = 1
        return
      end if
    endif
#endif
    
    if(.not.TCI)then
      do iCell = 1, this%nc
        f(iCell) = fi(this%ijxy(1,iCell),this%ijxy(2,iCell),1)
      enddo
      fo = matmul(this%recon_mtx, f)
      return
    endif
    
    do isub = 1, this%ns
      do iCell = 1,this%subs(isub)%nc
        f_sub(iCell,isub) = fi(this%subs(isub)%ijxy(1,iCell),this%subs(isub)%ijxy(2,iCell),1)
      enddo
    enddo
    
    ! Calculate point values from each available sub-stencils and smoothness
    ! indicators for those sub-stencils.
    do isub = 1, this%ns
      a(:,isub) = matmul(this%subs(isub)%iA, f_sub(1:this%subs(isub)%nc,isub))
      beta(isub) = this%subs(isub)%smooth_indicator(a(:,isub))
      fs(isub,:) = matmul(this%subs(isub)%poly, a(:,isub))
    end do

    alpha = weno_z_alpha(beta, this%ns, this%eps)

    do ipt = 1, this%npt
      ! Handle negative ideal coefficients by splitting method (Shi et al., 2002).
      alpha_p = this%rp(:,ipt) * alpha
      alpha_n = this%rn(:,ipt) * alpha
      fo(ipt) = this%sigmap(ipt) * sum(alpha_p * fs(:,ipt)) / sum(alpha_p) &
              - this%sigman(ipt) * sum(alpha_n * fs(:,ipt)) / sum(alpha_n)
    end do
      
  end subroutine weno_rhombus_reconstruct

  pure function weno_z_alpha(beta, ns, eps) result(res)

    real(8), intent(in) :: beta(ns)
    integer, intent(in) :: ns
    real(8), intent(in) :: eps
    real(8) res(ns)

    real(8) tau
    integer n, i, j

    tau = 0
    n   = 0
    do j = 1, ns - 1
      do i = j + 1, ns
        tau = tau + abs(beta(i) - beta(j))
        n = n + 1
      end do
    end do
    tau = tau / n

    !tau = (abs(beta(6) - beta(4)) + abs(beta(8) - beta(2)) + abs(beta(3) - beta(7)) + abs(beta(1) - beta(9))) / 4. 
    res = 1 + (tau / (beta + eps))**2

  end function weno_z_alpha
  
  pure logical function weno_rhombus_trouble_cell_indicator(this, fi, dx) result(res)

    class(recon_type), intent(in) :: this
    real(8), intent(in) :: fi(:,:,:)
    real(8), intent(in) :: dx

    integer i, j, k, m, n, ingb
    real(8), parameter :: sqrt2 = sqrt(2.0d0)
    real(8) c

    res = .false.
    k = 1
    do j = this%js, this%je
      do i = this%is, this%ie
        if (this%cell_ngb_size(i,j,k) > 0) then
          if (fi(i,j,k) == 0) then
            c = 2
          else
            c = 0
            do ingb = 1, this%cell_ngb_size(i,j,k)
              m = this%cell_ngb(1,ingb,i,j,k)
              n = this%cell_ngb(2,ingb,i,j,k)
              c = c + abs(fi(m,n,k) - fi(i,j,k))
            end do
            c = c / (this%cell_ngb_size(i,j,k) * sqrt2 * dx * abs(fi(i,j,k)))
          end if
          if (c >= 1) then
            res = .true.
            return
          end if
        end if
      end do
    end do

  end function weno_rhombus_trouble_cell_indicator
  
  subroutine weno_rhombus_reconstruct_deriv(this, fi, fx, fy, fz, ierr)

    class(recon_type), intent(inout) :: this
    real(8), intent(in ) :: fi(:,:,:) ! Cell averaged function values
    real(8), intent(out), optional :: fx(:)     ! Reconstructed x derivative values on evaluation points
    real(8), intent(out), optional :: fy(:)     ! Reconstructed y derivative values on evaluation points
    real(8), intent(out), optional :: fz(:)     ! Reconstructed y derivative values on evaluation points
    integer, intent(out), optional :: ierr

    integer i,j,k
    integer iCell
    
    real(8) f(this%nc)
    
#ifdef DEBUG
    if(present(ierr))then
      ierr = 0
      if (size(fi) /= this%nc) then
        if (present(ierr)) ierr = 1
        return
      end if
      if (size(fo) /= this%npt) then
        if (present(ierr)) ierr = 1
        return
      end if
    endif
#endif

    do iCell = 1, this%nc
      f(iCell) = fi(this%ijxy(1,iCell),this%ijxy(2,iCell),1)
    enddo
    fx = matmul(this%drecon_mtx(:,:,1,0,0), f)
    fy = matmul(this%drecon_mtx(:,:,0,1,0), f)
    
  end subroutine weno_rhombus_reconstruct_deriv

end module weno_rhombus_mod
